﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningPrimitives;

namespace MotionPlanning.MotionPlanningExperiments
{
    class Experiment_Multithreading : Experiment
    {
        private Thread[] workingThreads;

        public Experiment_Multithreading(string name, int n_threads, MotionPlanningAlgorithms.Planner_RRTBased planner, Form1 form, int iterations, int console_size = 30)
            : base(name, planner, form, iterations, console_size)
        {
            if (n_threads < 1)
                throw new Exception("Number of threads at least 1");
            workingThreads = new Thread[n_threads];
            for (int i = 0; i < workingThreads.Length; ++i)
            {
                workingThreads[i] = new Thread(new ThreadStart(experimentThreadFunction));
                workingThreads[i].Priority = ThreadPriority.Highest;
            }
        }

        override protected void experimentInstructions()
        {
            workingPlanner.Execute(iterations_requirement);
        }

        override protected void experimentThreadFunction()
        {
            experimentInstructions();
        }

        override public void Start()
        {
            visualThread.Start();
            monitoringThread.Start();
            for (int i = 0; i<workingThreads.Length;++i )
                if (workingThreads[i].ThreadState == System.Threading.ThreadState.Unstarted)
                    workingThreads[i].Start();
        }

        override public void Purge()
        {
            for (int i = 0; i<workingThreads.Length;++i )
                if (workingThreads[i].ThreadState != System.Threading.ThreadState.Unstarted)
                    workingThreads[i].Abort();
            if (visualThread.ThreadState != System.Threading.ThreadState.Unstarted)
                visualThread.Abort();
            if (monitoringThread.ThreadState != System.Threading.ThreadState.Unstarted)
                monitoringThread.Abort();
        }

        override protected void monitoringThreadFunction()
        {
            while (true)
            {
                monitor_msg.Clear();

                Trajectory solution = workingPlanner.GetSolution();
                if (solution != null)
                    monitor_msg.Append("cost = " + solution.GetCost() + "\n\n");

                int iteration = workingPlanner.Get_iterations_total();
                double progress = Math.Floor(iteration * 1000.0 / iterations_requirement / workingThreads.Length) / 10;
                monitor_msg
                    .Append("iteration = " + iteration)
                    .Append("\nvertices = " + workingPlanner.GetVertices().GetSize())
                    .Append("\nprogress = " + progress + "%")
                    .Append("\ntime(ms) = " + stopwatch.Elapsed.TotalMilliseconds)
                    .Append("\n")
                    .Append(workingPlanner.MonitorPlannerState());
                UIForm.BeginInvoke(new Action(monitorRefresh));

                int n_stop = 0;
                for(int i=0;i<workingThreads.Length;++i)
                {
                    if (stopwatch.IsRunning == false)
                        if (workingThreads[i].ThreadState == ThreadState.Running)
                            stopwatch.Start();
                    if (workingThreads[i].ThreadState == ThreadState.Stopped)
                        n_stop++;
                }
                if (n_stop == workingThreads.Length)
                    stopwatch.Stop();

                Thread.Sleep(100);
            }
        }
    }
}
